# -*- coding: utf-8 -*-
"""
Created on Tue Jan  4 21:57:49 2022

@author: saw
"""
import numpy as np
import matplotlib.pyplot as plt
from matplotlib import pyplot as plt
from matplotlib import colors

#仿照老师给的C++代码读取地图文件
MAGIC = np.uint32(0x15432345)
UINT_32_SIZE = 4
UINT_8_SIZE = 1
def load_map(addr):
    file = open(addr, "rb")
    my_magic = int.from_bytes(file.read(UINT_32_SIZE), byteorder='little', signed=False)
    if my_magic != MAGIC:
        return -1
    mx = int.from_bytes(file.read(UINT_32_SIZE), byteorder='little', signed=True)
    my = int.from_bytes(file.read(UINT_32_SIZE), byteorder='little', signed=True)
    _map = [[] for i in range(mx)]
    for i in range(0, mx * my):
        k = int(i / mx)
        _map[k].append(int.from_bytes(file.read(UINT_8_SIZE), byteorder='little', signed=True))
    m_start_x = int.from_bytes(file.read(UINT_32_SIZE), byteorder='little', signed=True)
    m_start_y = int.from_bytes(file.read(UINT_32_SIZE), byteorder='little', signed=True)
    m_end_x = int.from_bytes(file.read(UINT_32_SIZE), byteorder='little', signed=True)
    m_end_y = int.from_bytes(file.read(UINT_32_SIZE), byteorder='little', signed=True)

    file.close()
    return _map

#定义地图类 包括高h，宽w，数据data
class Maplist:
    def __init__(self,maplist):
        self.w=np.shape(maplist)[1]
        self.h=np.shape(maplist)[0]
        self.data=[]
        self.data=maplist
        
        #绘图主要用pcolor函数，起点，终点，障碍，路径分别对应不同的颜色
    def show_obstructBE(self,startpoint,endpoint):  #绘制当前地图
        self.data[startpoint.x][startpoint.y]=2
        self.data[endpoint.x][endpoint.y]=3
        cmap = colors.ListedColormap(['white','black','red','blue'])
        plt.figure(figsize=(6,6))
        plt.pcolor(self.data[::-1],cmap=cmap,linewidths=1)
        plt.axis('equal') 
        plt.axis('off') 
        plt.show()
    
    def show_path(self,point,i):
        self.data[point.x][point.y]=4
        cmap = colors.ListedColormap(['white','black','red','blue','green'])
        plt.figure(figsize=(6,6))
        plt.pcolor(self.data[::-1],cmap=cmap,linewidths=1)
        plt.axis('equal') 
        plt.axis('off') 
        #保存每帧图片，之后做成gif
        path='step2image_demo/'
        plt.savefig(path + str(i+1)+'.png')
        plt.show()

    def __getitem__(self, item):
        return self.data[item]
   

#定义点类，包含点的横纵坐标
class Point:
    def __init__(self,x,y):
        self.x=x;self.y=y 
    def __eq__(self, other):
        if self.x==other.x and self.y==other.y:
            return True
        return False

#定义A*算法主体，包括各函数，主循环
class AStar: 
    # 定义节点 可以完美和点区分开（point和node两类）
    class Node:  # 描述AStar算法中的节点数据
        def __init__(self, point, endPoint, g=0):
            self.point = point  # 当前节点的坐标
            self.parent = None  # 上一级节点
            self.g = g  # g()值
            # h()值,两坐标轴之差的绝对值的和
            self.h = (abs(endPoint.x - point.x) + abs(endPoint.y - point.y))   
 
    def __init__(self, nowmap, startPoint, endPoint,  obstruct=1):
        # 创建openlist和closelist
        self.openList = []
        self.closeList = []
        self.nowmap = nowmap  #导入地图
        #Astar中起点和终点
        self.startPoint = startPoint  
        self.endPoint = endPoint
        #定义地图上的障碍为obstrust=1
        self. obstruct =  obstruct

    #判断节点是否在closelist中
    def IsInCloseList(self, point):
        for node in self.closeList:
            if node.point == point:
                return True
        return False
    
    #判断节点是否在openlist中
    def IsInOpenList(self, point):
        for node in self.openList:
            if node.point == point:
                return node
        return None

    #找openlist中F最小的节点
    def getMinNode(self):
        minNode = self.openList[0]
        for node in self.openList:
            if node.g + node.h < minNode.g + minNode.h:
                minNode = node
        return minNode

    #判断终点是否在openlist中，作为最终结果依据：
    #把终点加入到了 open list 中，此时路径已经找到了，
    #或者查找终点失败，并且 open list 是空的，此时没有路径。
    def IsendPointInopenList(self):
        for node in self.openList:
            if node.point == self.endPoint:
                return node
        return None

    #计算当前点四周的节点数据（不允许斜着走）
    def Changenode(self, minFnode, dertaX, dertaY):
        # 当目标点碰到边界或是障碍或是在closelist中，不做考虑直接跳过
        if minFnode.point.x + dertaX < 0 or minFnode.point.x + dertaX > self.nowmap.w - 1 or minFnode.point.y + dertaY < 0 or minFnode.point.y + dertaY > self.nowmap.h - 1:
            return
        if self.nowmap[minFnode.point.x + dertaX][minFnode.point.y + dertaY] == self.obstruct:
            return
        nowPoint = Point(minFnode.point.x + dertaX, minFnode.point.y + dertaY)
        if self.IsInCloseList(nowPoint):
            return
                # 设置g()的变量(每次移动加1)
        derta_g = 1
        # 如果不再openList中，就把它加入openlist，并且把当前方格设置为它的上级节点，记录该方格的 F()，G() 和H()值。
        newNode = self.IsInOpenList(nowPoint)
        if not newNode:
            newNode = AStar.Node(nowPoint, self.endPoint, g=minFnode.g + derta_g)
            newNode.parent = minFnode
            self.openList.append(newNode)
            return
        
        # 如果在openList中，判断minFnode到当前点的G是否更小
        if minFnode.g + derta_g < newNode.g:  # 如果更小，就重新计算g值，并且改变上一级节点
            newNode.g = minFnode.g + derta_g
            newNode.parent = minFnode
 
    #算法主逻辑循环
    def start(self):
        # 1.将起点放入openlist
        startNode = AStar.Node(self.startPoint, self.endPoint)
        self.openList.append(startNode)
        # 2.主循环逻辑
        while True:
            # 遍历 openlist ，查找 F()值最小的节点，把它作为当前要处理的节点
            minFnode = self.getMinNode()
            # 把这个点加入closeList中，并且在openList中删除它
            self.closeList.append(minFnode)
            self.openList.remove(minFnode)
            # 判断这个节点的上下左右节点，这里的逻辑在Changenode()函数中已经写过
            self.Changenode(minFnode, 0, -1)
            self.Changenode(minFnode, 0, 1)
            self.Changenode(minFnode, -1, 0)
            self.Changenode(minFnode, 1, 0)
            # 判断是否终止
            resultpoint = self.IsendPointInopenList()
            if resultpoint != None:  # 如果终点在openlist中，则找到路径。
                finalPoint = resultpoint
                #创建pathlist，里面存放路径上的节点
                pathList = []
                #循环，倒推路径
                while 1:
                    if finalPoint.parent != None:
                        pathList.append(finalPoint.point)
                        finalPoint=finalPoint.parent
                    #找到起点，把路线反向传出，结束。    
                    if finalPoint.parent == None:                        
                        return list(reversed(pathList))

            # openlist是空的，此时没有路径        
            if len(self.openList) == 0:
                return None
            
if __name__ == '__main__':
    #读地图文件
    nowmap=Maplist(load_map("test_1.map"))
    #显示初始地图，包括障碍，起点，终点
    startpoint=Point(20, 43)
    endPoint=Point(83, 40)
    nowmap.show_obstructBE(startpoint,endPoint)
    #设置起点终点，寻路
    aStar=AStar(nowmap,startpoint,endPoint)
    #描绘路径，显示有路径的地图
    pathList=aStar.start()
   #i作为每帧图片名字
    i=0
    for point in pathList:
        ax=nowmap.show_path(point,i)
        i+=1


